#!/usr/bin/env python3
import os
import signal
import subprocess
import time
import serial
import rospy
from drone_pkg.msg import QRCode

x = -1
y = -1
data = -1
CamID = -1

def QRCode_Callback(QRCode):
    global x, y, data, CamID
    x = QRCode.x + QRCode.Width // 2
    y = QRCode.y + QRCode.Height // 2
    data = int(QRCode.Data)
    CamID = QRCode.CamID
    string = str(data) + "," + str(x) + "," + str(y) + "," + str(CamID) + "\r\n"
    print(string)
    ser.write(string.encode())

rospy.init_node("QRCode_test", anonymous=True)
sub = rospy.Subscriber("/QRCode_detect", QRCode, QRCode_Callback, queue_size=10)
ser = serial.Serial("/dev/ttyS3", 115200)
rospy.spin()

# # 启动 roslaunch
# ros_process = subprocess.Popen(["roslaunch", "cartographer_ros", "lidar.launch"])

# a = int(input("请输入数字："))


# if a == 1:
#     print("结束 ROS 程序...")
#     ros_process.terminate() 
#     time.sleep(2) 
#     if ros_process.poll() is None:
#         ros_process.kill()
#     print("ROS 程序已终止。")

# ser = serial.Serial("/dev/ttyS3", 115200)
# while ser.read(1).decode()!='a':
#             time.sleep(0.01)
# ser.write("AAA".encode())
# print("OK")






# while True:
#     os.system("gpio write 10 0")
#     time.sleep(1)
#     os.system("gpio write 10 1")
#     time.sleep(1)






